#pragma config(Sensor, S4,     HTANG_LEFT,     sensorI2CCustom)
#pragma config(Sensor, S1,     HTANG_RIGHT,    sensorI2CCustom)
#pragma config(Sensor, S2,     TOUCH_SENSOR,    sensorTouch)
#pragma config(Sensor, S3,     SONAR,          sensorSONAR)
#pragma config(Motor,  motorC,          MOTOR_LEFT,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorA,          MOTOR_RIGHT,   tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          MOTOR_PUSH,   tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#pragma platform(NXT)


#ifndef _IADECISIONMAKER__
#define _IADECISIONMAKER__ // prevent multiple compilations

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: ROBOTC PROJECT MAIN FILE DECLARATION
#ifndef PROJECT_MAIN_FILE
#define PROJECT_MAIN_FILE "iaDecisionMaker.c" //!< enable compilation of task main() and various debug and test functions of this file
#endif
// END OF:  ROBOTC PROJECT MAIN FILE DECLARATION
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: HEADERS
#include "iaDecisionMaker.h"
// END OF:  HEADERS
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: GLOBAL INSTANCES
#ifndef M_2PI
#define M_2PI	(2*PI)
#endif
// END OF:  GLOBAL INSTANCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: EXTERNAL SOURCES
#include "pmInit.c"
#include "pmSensors.c"
// END OF:  EXTERNAL SOURCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: LOCAL INSTANCES

// Management of the actions list
int ia_actions_max = 50;
ACTION_TYPE ia_actions_array_types[50];
float ia_actions_array_values1[50];
float ia_actions_array_values2[50];
int ia_actions_current_max = 0;
int ia_actions_current_index = 0;
bool ia_actions_isEmpty = true;
bool ia_actions_end = false;

// Management of the decisions
IADecision ia_decisions_array[10];
int ia_point_decisions_array[6][5];
int currentDecisionId = -1;

// Management of the zone parser
bool zoneAnalysed[6];

// END OF:  LOCAL INSTANCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: CODES

//================================================================================================================================
// Initialisation step
//================================================================================================================================


// INIT
void dm_init()
{
	// Init decisions

	// presents
	dm_init_present(0);
	dm_init_present(1);
	dm_init_present(2);
	dm_init_present(3);
	// glasses on blue side
	dm_init_glass(4);
	dm_init_glass(5);
	dm_init_glass(6);
	// glasses on red side
	dm_init_glass(7);
	dm_init_glass(8);
	dm_init_glass(9);

	// Init decisions for points
	if (pmInitIA == PM_INIT_IA_MATCH)
	{
		ia_point_decisions_array[0][0] = 0;
		ia_point_decisions_array[0][1] = 1;
		ia_point_decisions_array[0][2] = -1;
		ia_point_decisions_array[0][3] = -1;
		ia_point_decisions_array[0][4] = -1;

		ia_point_decisions_array[1][0] = 1;
		ia_point_decisions_array[1][1] = 2;
		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[1][2] = 4;
			ia_point_decisions_array[1][3] = 5;
			ia_point_decisions_array[1][4] = -1;
		} else {
			ia_point_decisions_array[1][2] = 7;
			ia_point_decisions_array[1][3] = 8;
			ia_point_decisions_array[1][4] = -1;
		}

		ia_point_decisions_array[2][0] = 3;
		ia_point_decisions_array[2][1] = 2;
		ia_point_decisions_array[2][2] = -1;
		ia_point_decisions_array[2][3] = -1;
		ia_point_decisions_array[2][4] = -1;

		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[3][0] = -1;
			ia_point_decisions_array[3][1] = -1;
		} else {
			ia_point_decisions_array[3][0] = 6;
			ia_point_decisions_array[3][1] = 5;
		}
		ia_point_decisions_array[3][2] = -1;
		ia_point_decisions_array[3][3] = -1;
		ia_point_decisions_array[3][4] = -1;

		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[4][0] = 6;
			ia_point_decisions_array[4][1] = 5;
		} else {
			ia_point_decisions_array[4][0] = 9;
			ia_point_decisions_array[4][1] = 8;
		}
		ia_point_decisions_array[4][2] = -1;
		ia_point_decisions_array[4][3] = -1;
		ia_point_decisions_array[4][4] = -1;

		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[5][0] = 6;
			ia_point_decisions_array[5][1] = 5;
		} else {
			ia_point_decisions_array[5][0] = -1;
			ia_point_decisions_array[5][1] = -1;
		}
		ia_point_decisions_array[5][2] = -1;
		ia_point_decisions_array[5][3] = -1;
		ia_point_decisions_array[5][4] = -1;

	} else if(pmInitIA == PM_INIT_IA_HOMOLOG) {
		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[0][0] = 0;
			ia_point_decisions_array[0][1] = 1;
		} else {
			ia_point_decisions_array[0][0] = 0;
			ia_point_decisions_array[0][1] = -1;
		}
		ia_point_decisions_array[0][2] = -1;
		ia_point_decisions_array[0][3] = -1;
		ia_point_decisions_array[0][4] = -1;
		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[1][0] = 1;
			ia_point_decisions_array[1][1] = 4;
			ia_point_decisions_array[1][2] = 2;
		} else {
			ia_point_decisions_array[1][0] = 2;
			ia_point_decisions_array[1][1] = 7;
			ia_point_decisions_array[1][2] = 1;
		}
		ia_point_decisions_array[1][3] = -1;
		ia_point_decisions_array[1][4] = -1;

		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			ia_point_decisions_array[2][0] = 3;
			ia_point_decisions_array[2][1] = -1;
		} else {
			ia_point_decisions_array[2][0] = 3;
			ia_point_decisions_array[2][1] = 2;
		}
		ia_point_decisions_array[2][2] = -1;
		ia_point_decisions_array[2][3] = -1;
		ia_point_decisions_array[2][4] = -1;

		ia_point_decisions_array[3][0] = -1;
		ia_point_decisions_array[3][1] = -1;
		ia_point_decisions_array[3][2] = -1;
		ia_point_decisions_array[3][3] = -1;
		ia_point_decisions_array[3][4] = -1;

		ia_point_decisions_array[4][0] = -1;
		ia_point_decisions_array[4][1] = -1;
		ia_point_decisions_array[4][2] = -1;
		ia_point_decisions_array[4][3] = -1;
		ia_point_decisions_array[4][4] = -1;

		ia_point_decisions_array[5][0] = -1;
		ia_point_decisions_array[5][1] = -1;
		ia_point_decisions_array[5][2] = -1;
		ia_point_decisions_array[5][3] = -1;
		ia_point_decisions_array[5][4] = -1;
	}
}

// Creating the present decision
void dm_init_present(int index)
{
	ia_decisions_array[index].done = false;
	float posX;
	switch (index)
	{
		case 0:
			posX = 0.6;
			break;
		case 1:
			posX = 1.2;
			break;
		case 2:
			posX = 1.8;
			break;
		case 3:
			posX = 2.4;
			break;
	}
	if (pmInitColor == PM_INIT_COLOR_BLUE) {
		posX = posX - 0.08;
	} else {
		posX = posX + 0.08;
	}
	ia_decisions_array[index].firstPointX = posX;
	ia_decisions_array[index].firstPointY = 1.8;
	ia_decisions_array[index].firstPointA = PI / 2;

	// Preparing the final actions
	ia_decisions_array[index].finalActionsSize = 4;

	ia_decisions_array[index].finalActions[0].type = ACTION_TYPE_PUSH;
	ia_decisions_array[index].finalActions[0].value1 = 0;

	ia_decisions_array[index].finalActions[1].type = ACTION_TYPE_MOVE_NO_DETECT;
	ia_decisions_array[index].finalActions[1].value1 = 0.17;

	ia_decisions_array[index].finalActions[2].type = ACTION_TYPE_PUSH;
	ia_decisions_array[index].finalActions[2].value1 = 1;

	ia_decisions_array[index].finalActions[3].type = ACTION_TYPE_MOVE_NO_DETECT;
	ia_decisions_array[index].finalActions[3].value1 = -0.17;

	//ia_decisions_array[index].finalActions[4].type = ACTION_TYPE_TURN_TO_ANGLE;
	//ia_decisions_array[index].finalActions[4].value1 = -PI/2;
	//ia_decisions_array[index].finalActions[4].type = ACTION_TYPE_TURN_AND_MOVE;
	//ia_decisions_array[index].finalActions[4].value1 = posX;
	//ia_decisions_array[index].finalActions[4].value2 = 1.8;

}

void dm_init_glass(int index)
{
	ia_decisions_array[index].done = false;
	float posX, posY, dist;
	switch (index)
	{
		case 4:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 1.5;
				posY = 1.45;
			} else {
				posX = 0.5;
				posY = 1.45;
			}
			break;
		case 5:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 1.5;
				posY = 1.4;
			} else {
				posX = 0.5;
				posY = 1.0;
			}
			break;
		case 6:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 1.5;
				posY = 0.75;
			} else {
				posX = 0.5;
				posY = 0.75;
			}
			break;
		case 7:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 2.5;
				posY = 1.45;
			} else {
				posX = 1.5;
				posY = 1.45;
			}
			break;
		case 8:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 2.5;
				posY = 1.0;
			} else {
				posX = 1.5;
				posY = 1.4;
			}
			break;
		case 9:
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				posX = 2.5;
				posY = 0.75;
			} else {
				posX = 1.5;
				posY = 0.75;
			}
			break;

	}
	ia_decisions_array[index].firstPointX = posX;
	ia_decisions_array[index].firstPointY = posY;
	if (pmInitColor == PM_INIT_COLOR_BLUE) {
		ia_decisions_array[index].firstPointA = PI;
	} else {
	  ia_decisions_array[index].firstPointA = 0;
	}
	ia_decisions_array[index].finalActionsSize = 2;

	ia_decisions_array[index].finalActions[0].type = ACTION_TYPE_MOVE;
	if (pmInitColor == PM_INIT_COLOR_BLUE) {
		ia_decisions_array[index].finalActions[0].value1 = posX - 0.4;
	} else {
		ia_decisions_array[index].finalActions[0].value1 = 2.6 - posX;
	}
	ia_decisions_array[index].finalActions[1].type = ACTION_TYPE_MOVE_NO_DETECT;
	ia_decisions_array[index].finalActions[1].value1 = -0.1;
}

//================================================================================================================================
// Decision master and tools
//================================================================================================================================

// Make a decision
void dm_takeDecision(float x1, float y1, float theta1)
{
	// Clears the actions pool
	dm_actionsClear();
	// Get position
	writeDebugStreamLine("iaDecisionMaker.c : Taking decision. Position: x=%f  y=%f  theta=%f", x1, y1, theta1);
	switch(pmInitIA){
		case PM_INIT_IA_TEST:
			writeDebugStreamLine("iaDecisionMaker.c : IA = PM_INIT_IA_TEST");
			dm_testDecide(x1, y1, theta1);
			break;
		case PM_INIT_IA_TABLE_TEST:
			writeDebugStreamLine("iaDecisionMaker.c : IA = PM_INIT_IA_TABLE_TEST");
			dm_tableTestDecide(x1, y1, theta1);
			break;
		case PM_INIT_IA_HOMOLOG:
			writeDebugStreamLine("iaDecisionMaker.c : IA = PM_INIT_IA_HOMOLOG");
			dm_matchDecide(x1, y1, theta1);
			break;
		case PM_INIT_IA_MATCH:
			writeDebugStreamLine("iaDecisionMaker.c : IA = PM_INIT_IA_MATCH");
			dm_matchDecide(x1, y1, theta1);
			break;
	}
}

//================================================================================================================================
// Match decisions
//================================================================================================================================

// Match decisions
void dm_matchDecide(float x1, float y1, float theta1)
{
	writeDebugStreamLine("iaDecisionMaker.c : Match decision");
	// Finding the zone
	int currentZone = getCurrentZone(x1, y1, theta1);
	// Keep current zone for further path
	int zone = currentZone;
	writeDebugStreamLine("   Robot in zone %i", zone);
	// Getting the next decision
	bool todoFound = false;
	for (int index = 0; index < 6; index++)
		zoneAnalysed[index] = false;
	int id_todo;
	while (zone != -1 && !todoFound)
	{
		writeDebugStreamLine("   Analysing zone %i", zone);
		for (int index = 0; index < 5 && !todoFound; index++)
		{
			id_todo = ia_point_decisions_array[zone][index];
			if (id_todo != -1)
			{
				writeDebugStreamLine("   Todo %i action found. First point is %f, %f", id_todo, ia_decisions_array[id_todo].firstPointX, ia_decisions_array[id_todo].firstPointY);
				if (!ia_decisions_array[id_todo].done)
				{
					if (!sensorFrontOponentDetected || !dm_isOponentInFirstDirection(x1, y1, theta1, ia_decisions_array[id_todo].firstPointX, ia_decisions_array[id_todo].firstPointY))
					{
						writeDebugStreamLine("   Todo action correct");
						todoFound = true;
					} else {
						writeDebugStreamLine("   Oponent found in the direction of the first point, finding another decision");
					}
				} else {
					writeDebugStreamLine("   Action already done, finding another one");
				}
			}
		}
		zoneAnalysed[zone] = true;
		if (!todoFound)
		{
			// No decision found, moving to closest interesting point
			zone = getNextInterrestingZone();
		}
	}
	// If a decision was taken
	if (todoFound)
	{
		currentDecisionId = id_todo;
		if (zone != currentZone)
		{
			writeDebugStreamLine("   Move robot to zone %i", zone);
			// Move to new zone
			IAAction moveToZone;
			moveToZone.type = ACTION_TYPE_TURN_AND_MOVE;
			if (zone == 0)
			{
				moveToZone.value1 = 0.6;
				moveToZone.value2 = 1.75;
			} else if(zone == 1) {
				moveToZone.value1 = 1.5;
				moveToZone.value2 = 1.75;
			} else if(zone == 2) {
				moveToZone.value1 = 2.4;
				moveToZone.value2 = 1.75;
			} else if(zone == 3) {
				moveToZone.value1 = 0.6;
				moveToZone.value2 = 0.75;
			} else if(zone == 4) {
				moveToZone.value1 = 1.5;
				moveToZone.value2 = 0.75;
			} else if(zone == 5) {
				moveToZone.value1 = 2.4;
				moveToZone.value2 = 0.75;
			}
			dm_actionsAdd(moveToZone);
		}
		writeDebugStreamLine("   Moving to point x=%f, y=%f", ia_decisions_array[id_todo].firstPointX, ia_decisions_array[id_todo].firstPointY);
		IAAction turnAndMove;
		turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
		turnAndMove.value1 = ia_decisions_array[id_todo].firstPointX;
		turnAndMove.value2 = ia_decisions_array[id_todo].firstPointY;
		dm_actionsAdd(turnAndMove);
		// Turn to the target angle
		IAAction turnToAngle;
		turnToAngle.type = ACTION_TYPE_TURN_TO_ANGLE;
		turnToAngle.value1 = ia_decisions_array[id_todo].firstPointA;
		dm_actionsAdd(turnToAngle);
		// Do the final actions
		for (int indexFinal = 0; indexFinal < ia_decisions_array[id_todo].finalActionsSize; indexFinal++)
		{
			writeDebugStreamLine("   Final action %i type %i value %f, %f", indexFinal, ia_decisions_array[id_todo].finalActions[indexFinal].type, ia_decisions_array[id_todo].finalActions[indexFinal].value1, ia_decisions_array[id_todo].finalActions[indexFinal].value2);
			dm_actionsAdd(ia_decisions_array[id_todo].finalActions[indexFinal]);
		}
	} else {
		writeDebugStreamLine("   No remaining action to do");
	}
}

bool dm_isOponentInFirstDirection(float x1, float y1, float theta1, float x2, float y2)
{
	float directionAngle = theta1 - dm_math_atan2f(y2 - y1, x2 - x1);
	if(directionAngle < -PI)
		directionAngle = directionAngle+M_2PI;
	else if(directionAngle > PI)
		directionAngle = directionAngle-M_2PI;
	if (directionAngle > -criticalAngleForOponent && directionAngle < criticalAngleForOponent)
	{
		return true;
	} else {
		return false;
	}
}

int getCurrentZone(float x1, float y1, float theta1)
{
	int zone = -1;
	if (x1 < 1.05)
		zone = 0;
	else if (x1 < 1.95)
		zone = 1;
	else
		zone = 2;
	if (y1 < 1.2)
		zone += 3;
	writeDebugStreamLine("iaDecisionMaker.c : Current zone (%f, %f): zone %i", x1, y1, zone);
	return zone;
}

int getNextInterrestingZone()
{
	int bestZone = -1;
	int bestZonePoints = 0;
	IADecision todo;
	for (int absoluteZone = 0; absoluteZone < 6; absoluteZone++)
	{
		int zone = absoluteZone;
		if (pmInitColor == PM_INIT_COLOR_RED) {
			if (zone == 0){
				zone = 2;
			} else if (zone == 2){
				zone = 0;
			} else if (zone == 3){
				zone = 5;
			} else if (zone == 5){
				zone = 3;
			}
		}

		if (!zoneAnalysed[zone])
		{
			int zonePoints = 0;
			for (int index = 0; index < 5; index++)
			{
				int id = ia_point_decisions_array[zone][index];
				if (id != -1)
				{
					todo = ia_decisions_array[id];
					if (!todo.done)
					{
						zonePoints++;
					}
				}
			}
			if (zonePoints > bestZonePoints)
			{
				bestZone = zone;
				bestZonePoints = zonePoints;
			}
		}
	}
	writeDebugStreamLine("iaDecisionMaker.c : Best zone found: %i, todos %i", bestZone, bestZonePoints);
	return bestZone;
}

//================================================================================================================================
// Test Table decisions
//================================================================================================================================

// Testing decisions (doing an eight)
int tableTestDecision = 0;
void dm_tableTestDecide(float x1, float y1, float theta1)
{
	writeDebugStreamLine("iaDecisionMaker.c : Table test decision, step %i", tableTestDecision);
	IAAction turnAndMove;
	switch(tableTestDecision){
		case 0:
			tableTestDecision = 1;
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 0.6;
				turnAndMove.value2 = 1.7;
				dm_actionsAdd(turnAndMove);
			} else {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 2.4;
				turnAndMove.value2 = 1.7;
				dm_actionsAdd(turnAndMove);
			}
			break;
		case 1:
			tableTestDecision = 2;
			turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
			turnAndMove.value1 = 1.5;
			turnAndMove.value2 = 1.7;
			dm_actionsAdd(turnAndMove);
			break;
		case 2:
			tableTestDecision = 3;
			turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
			turnAndMove.value1 = 1.5;
			turnAndMove.value2 = 0.7;
			dm_actionsAdd(turnAndMove);
			break;
		case 3:
			tableTestDecision = 4;
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 2.4;
				turnAndMove.value2 = 0.7;
				dm_actionsAdd(turnAndMove);
			} else {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 0.6;
				turnAndMove.value2 = 0.7;
				dm_actionsAdd(turnAndMove);
			}
			break;
		case 4:
			tableTestDecision = 5;
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 2.4;
				turnAndMove.value2 = 1.7;
				dm_actionsAdd(turnAndMove);
			} else {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 0.6;
				turnAndMove.value2 = 1.7;
				dm_actionsAdd(turnAndMove);
			}
			break;
		case 5:
			tableTestDecision = 6;
			turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
			turnAndMove.value1 = 1.5;
			turnAndMove.value2 = 1.7;
			dm_actionsAdd(turnAndMove);
			break;
		case 6:
			tableTestDecision = 7;
			turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
			turnAndMove.value1 = 1.5;
			turnAndMove.value2 = 0.7;
			dm_actionsAdd(turnAndMove);
			break;
		case 7:
			tableTestDecision = 0;
			if (pmInitColor == PM_INIT_COLOR_BLUE) {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 0.6;
				turnAndMove.value2 = 0.7;
				dm_actionsAdd(turnAndMove);
			} else {
				turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
				turnAndMove.value1 = 2.4;
				turnAndMove.value2 = 0.7;
				dm_actionsAdd(turnAndMove);
			}
			break;
	}
}

//================================================================================================================================
// Custom test decisions
//================================================================================================================================

bool testAlreadyDone = false;
void dm_testDecide(float x1, float y1, float theta1)
{
	writeDebugStreamLine("iaDecisionMaker.c : Simple test decision");
	if (!testAlreadyDone) {
		/*
		IAAction move1;
		move1.type = ACTION_TYPE_MOVE;
		move1.value = -0.2;
		dm_actionsAdd(move1);
		IAAction turn1;
		turn1.type = ACTION_TYPE_TURN;
		turn1.value = PI;
		dm_actionsAdd(turn1);
		IAAction down;
		down.type = ACTION_TYPE_PUSH;
		down.value = 0;
		dm_actionsAdd(down);
		IAAction move;
		move.type = ACTION_TYPE_MOVE_NO_DETECT;
		move.value = -0.2;
		dm_actionsAdd(move);
		IAAction up;
		up.type = ACTION_TYPE_PUSH;
		up.value = 1;
		dm_actionsAdd(up);
		IAAction moveBack;
		moveBack.type = ACTION_TYPE_MOVE;
		moveBack.value = 0.2;
		dm_actionsAdd(moveBack);
		*/
		/*
		IAAction turn1;
		turn1.type = ACTION_TYPE_TURN;
		turn1.value = PI;
		dm_actionsAdd(turn1);
		*/
		/*
		IAAction move;
		move.type = ACTION_TYPE_MOVE;
		move.value1 = -0.2;
		dm_actionsAdd(move);
		*/
		// Base position for blue
		//turnAndMove.value1 = 0.079;
		//turnAndMove.value2 = 1.684;
		// Base position for red
		//turnAndMove.value1 = 2.921
		//turnAndMove.value2 = 1.684;
		// Base for blue
		IAAction turnAndMove;
		turnAndMove.type = ACTION_TYPE_TURN_AND_MOVE;
		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			turnAndMove.value1 = 0.579;
		} else {
			turnAndMove.value1 = 2.421;
		}
		turnAndMove.value2 = 1.684;
		dm_actionsAdd(turnAndMove);
		IAAction turnAndMove2;
		turnAndMove2.type = ACTION_TYPE_TURN_AND_MOVE;
		if (pmInitColor == PM_INIT_COLOR_BLUE) {
			turnAndMove2.value1 = 0.079;
		} else {
			turnAndMove2.value1 = 2.921;
		}
		turnAndMove2.value2 = 1.684;
		dm_actionsAdd(turnAndMove2);
		dm_actionsAdd(turnAndMove);
		dm_actionsAdd(turnAndMove2);
		dm_actionsAdd(turnAndMove);
		dm_actionsAdd(turnAndMove2);
		// Kill
		testAlreadyDone = true;
	}
}

//================================================================================================================================
// ACTIONS pool management part
//================================================================================================================================


void dm_actionsNext(ACTION_TYPE* action_type, float* action_value, float* action_turn_value, float* action_move_value, float x1, float y1, float theta1)
{
	if (ia_actions_current_index >= ia_actions_current_max)
	{
		writeDebugStreamLine("iaDecisionMaker.c : No more action, index = %i, maxIndex = %i", ia_actions_current_index, ia_actions_current_max);
		*action_type = ACTION_TYPE_END;
	} else {
		writeDebugStreamLine("iaDecisionMaker.c : Getting next action, index = %i", ia_actions_current_index);
		*action_type = ia_actions_array_types[ia_actions_current_index];
		/*
		*action_value = ia_actions_array_values1[ia_actions_current_index];
		*/
		if (*action_type == ACTION_TYPE_TURN_AND_MOVE || *action_type == ACTION_TYPE_TURN_AND_MOVE_NO_DETECT)
		{
			float angle;
			float dist;
			dm_computeAlphaDelta(x1, y1, theta1, ia_actions_array_values1[ia_actions_current_index], ia_actions_array_values2[ia_actions_current_index], &angle, &dist);
			writeDebugStreamLine("iaDecisionMaker.c : Turn %f, advance %f", angle, dist);
			// Turns the robot in the good direction
			*action_turn_value = angle;
			// Move to the point. If dist > 0.5, let 0.2 for correction
			if (dist < 0.5)
			{
				*action_move_value = dist;
			} else {
				*action_move_value = dist - 0.2;
				// Inserting new action before next step
				IAAction action;
				action.type = ia_actions_array_types[ia_actions_current_index];
				action.value1 = ia_actions_array_values1[ia_actions_current_index];
				action.value2 = ia_actions_array_values2[ia_actions_current_index];
				dm_actionsInsert(action);
			}
		} else if (*action_type == ACTION_TYPE_TURN_TO_ANGLE) {
			float moveAngle = ia_actions_array_values1[ia_actions_current_index] - theta1;
			if(moveAngle < -PI)
				moveAngle = moveAngle+M_2PI;
			else if(moveAngle > PI)
				moveAngle = moveAngle-M_2PI;
			*action_value = moveAngle;
		} else {
			*action_value = ia_actions_array_values1[ia_actions_current_index];
		}
	}
}

void dm_actionsFinishCurrentAction()
{
	ia_actions_current_index++;
}

void dm_actionsFinishCurrentDecision()
{
	if (pmInitIA == PM_INIT_IA_HOMOLOG || pmInitIA == PM_INIT_IA_MATCH)
	{
		if (currentDecisionId != -1)
		{
			ia_decisions_array[currentDecisionId].done = true;
		}
	}
}

void dm_actionsClear()
{
	writeDebugStreamLine("iaDecisionMaker.c : Clearing the movements pile");
	ia_actions_isEmpty = true;
	ia_actions_current_max = 0;
	ia_actions_current_index = 0;
}

bool dm_actionsAdd(IAAction action)
{
	if (ia_actions_current_max >= ia_actions_max)
	{
		writeDebugStreamLine("ERROR iaDecisionMaker.c : Stack overflow, index = %i, indexMax = %i", ia_actions_current_max, ia_actions_max);
		return false;
	} else {
		ia_actions_isEmpty = false;
		ia_actions_array_types[ia_actions_current_max] = action.type;
		ia_actions_array_values1[ia_actions_current_max] = action.value1;
		ia_actions_array_values2[ia_actions_current_max] = action.value2;
		writeDebugStreamLine("iaDecisionMaker.c : Adding movement in pile, index = %i", ia_actions_current_max);
		/*
		if (action.type == ACTION_TYPE_MOVE)
		{
			writeDebugStreamLine("   Move %f", action.value1);
		} else if (action.type == ACTION_TYPE_MOVE_NO_DETECT) {
			writeDebugStreamLine("   Move %f (no detect)", action.value1);
		} else if (action.type == ACTION_TYPE_TURN) {
			writeDebugStreamLine("   Turn %f", action.value1);
		} else if (action.type == ACTION_TYPE_TURN_TO_ANGLE) {
			writeDebugStreamLine("   Turn to %f", action.value1);
		} else if (action.type == ACTION_TYPE_PUSH) {
			writeDebugStreamLine("   Push %f", action.value1);
		} else if (action.type == ACTION_TYPE_TURN_AND_MOVE) {
			writeDebugStreamLine("   Turn and Move to %f, %f", action.value1, action.value2);
		} else if (action.type == ACTION_TYPE_TURN_AND_MOVE_NO_DETECT) {
			writeDebugStreamLine("   Turn and Move to %f, %f (no detection)", action.value1, action.value2);
		} else {
			writeDebugStreamLine("   Unknown");
		}
		*/
		ia_actions_current_max++;
		return true;
	}
}

bool dm_actionsInsert(IAAction action)
{
	if (ia_actions_current_max >= ia_actions_max)
	{
		writeDebugStreamLine("ERROR iaDecisionMaker.c : Stack overflow, index = %i, indexMax = %i", ia_actions_current_max, ia_actions_max);
		return false;
	} else {
		ia_actions_isEmpty = false;
		for (int actionIndex = ia_actions_current_max+1; actionIndex > ia_actions_current_index + 1; actionIndex--)
		{
			ia_actions_array_types[actionIndex] = ia_actions_array_types[actionIndex-1];
			ia_actions_array_values1[actionIndex] = ia_actions_array_values1[actionIndex-1];
			ia_actions_array_values2[actionIndex] = ia_actions_array_values2[actionIndex-1];
		}
		ia_actions_array_types[ia_actions_current_index+1] = action.type;
		ia_actions_array_values1[ia_actions_current_index+1] = action.value1;
		ia_actions_array_values2[ia_actions_current_index+1] = action.value2;
		writeDebugStreamLine("iaDecisionMaker.c : Inserting movement in pile, index = %i", ia_actions_current_max);
		ia_actions_current_max++;
		return true;
	}
}

//================================================================================================================================
// MATHS parts
//================================================================================================================================
// Copied from crobotTrajWrappers

float dm_computeAlphaDelta(float x1, float y1, float theta1, float x2, float y2, float *out_angle, float *out_dist)
{
	writeDebugStreamLine("Compute movement from %f, %f, %f, to %f, %f", x1, y1, theta1, x2, y2);
	float dx,dy;
	float dTheta;

	dx = x2 - x1;
	dy = y2 - y1;

	//s'il n'y a pas de deplacement a faire,
	//et bien on en fait pas...
	if(dx == 0.0 && dy == 0.0)
	{
		*out_angle = 0.0;
		*out_dist = 0.0;
		return theta1;
	}

	//angle to run to reach position
	//dTheta = atan2f(dy,dx) - theta1;
	float newAngle = dm_math_atan2f(dy,dx);
	dTheta = dm_math_atan2f(dy,dx) - theta1;

	//bound angle value to [-Pi ; Pi]
	if(dTheta < -PI)
		*out_angle = dTheta+M_2PI;
	else if(dTheta > PI)
		*out_angle = dTheta-M_2PI;
	else
		*out_angle = dTheta;

	//distance to run
	*out_dist = sqrt(dx*dx+dy*dy);

	writeDebugStreamLine("  resulting angle=%f, dist=%f", *out_angle, *out_dist);

	// Returns the new angle
	if(newAngle < -PI)
		newAngle = newAngle+M_2PI;
	else if(newAngle > PI)
		newAngle = newAngle-M_2PI;
	return newAngle;
}

float dm_math_atan2f(float y, float x)
{
	float absx, absy, val;
	if (x == 0 && y == 0)
	{
		return 0;
	}
	absy = y < 0 ? -y : y;
	absx = x < 0 ? -x : x;
	if (absy - absx == absy)
	{
		/* x negligible compared to y */
		return y < 0 ? -(PI/2) : (PI/2);
	}
	if (absx - absy == absx) {
		/* y negligible compared to x */
		val = 0.0;
	}
	else    val = atan(y/x);
	if (x > 0)
	{
		/* first or fourth quadrant; already correct */
		return val;
	}
	if (y < 0)
	{
		/* third quadrant */
		return val - PI;
	}
	return val + PI;
}

// END OF:  CODES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: TESTS
#if(PROJECT_MAIN_FILE == "iaDecisionMaker.c")
	//-----------------------------------------------------------------------------------------------------------------------------
	//! @brief     this task tests the functions
	//! @return    none
	//! @author    cho
	//! @note
	//! @todo
	//-----------------------------------------------------------------------------------------------------------------------------
	task main()
	{
		dm_init();
	}
#endif // (PROJECT_MAIN_FILE)

// END OF:  TESTS
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

#endif
